#include <ros/ros.h>
#include <serial/serial.h>
#include <geometry_msgs/Twist.h>
#include <cstring>

//创建串口变量
serial::Serial ser;

size_t n = 2;//定义发送数组的前几个字节

//定义头尾帧
uint8_t head[3] = {0x0A,0x0F,0x00};
uint8_t hip[3] = {0x0F,0x0A,0x00};

//打开串口
void openserial()
	{
		try
			{
				ser.setPort("/dev/ttyUSB0");//设备端口号
				ser.setBaudrate(38400);//波特率
				serial::Timeout t = serial::Timeout::simpleTimeout(1000);//这个应该是超时，但是是必须的！！ 
      			ser.setTimeout(t); 
				ser.open();//打开串口
			}
		catch(serial::IOException& e) 
            { 
                ROS_ERROR_STREAM("Unable to open /dev/ttyUSB0 "); 
            } 
		if(ser.isOpen())
			{
				ROS_INFO("OPEN");
			}
	}

//订阅到cmd_vel后的回调函数
void vel_callback(const geometry_msgs::Twist::ConstPtr &msg)
	{
        ser.write(head,n);

        float vel[2];
        vel[0] = msg->linear.x;
        vel[1] = msg->linear.y;
        // vel[0] = _Float32(msg.linear.x);
        // vel[1] = _Float32(msg.linear.y);
        // vel[0] = static_cast<float>(msg.linear.x);
        // vel[1] = static_cast<float>(msg.linear.y);
        uint8_t byte_arry[sizeof(vel)];
        memcpy(byte_arry, vel, sizeof(vel));
        ser.write(byte_arry, sizeof(vel));

        ser.write(hip,n);
        ROS_INFO("已发...");
	}


int main(int argc,char **argv)
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"talker_stm32");
    ros::NodeHandle m;
    openserial();//打开串口
    ros::Subscriber vel_sub = m.subscribe("/cmd_vel",100,vel_callback);

    while(ros::ok())
    {
        ros::spinOnce();
    }

    return 0;
}
